#include <Servo.h> 
#include <Wire.h>
#include <ArduinoNunchuk.h>

Servo myServoTete;
Servo myServoBrasDroit;
Servo myServoBrasGauche;

#define BAUDRATE 19200

#define SERVO_BRAS_DROIT 7
#define SERVO_BRAS_GAUCHE 5
#define SERVO_TETE 6

#define LED_GAUCHE 9
#define LED_DROITE 8
#define LED_PECTORAL 10
ArduinoNunchuk nunchuk = ArduinoNunchuk();

#define MIN_JOY 0
#define MAX_JOY 255

#define MIN_SERVO 1
#define MAX_SERVO 181

int oldAngleTete = 0;
int oldAngleBrasDroit = 0;
int oldAngleBrasGauche = 0;

int oldStatusBoutonZ = 0;

void setup ()
{
  Serial.begin(BAUDRATE);
  nunchuk.init();
  myServoTete.attach( SERVO_TETE );
  myServoBrasDroit.attach( SERVO_BRAS_DROIT );
  myServoBrasGauche.attach( SERVO_BRAS_GAUCHE );
  
  pinMode( LED_GAUCHE, OUTPUT);
  pinMode( LED_DROITE, OUTPUT);  
  pinMode( LED_PECTORAL, OUTPUT);  
}

void loop ()
{
  nunchuk.update();
  int angleTete = (int) (( (float) nunchuk.analogX / ( MAX_JOY - MIN_JOY ) ) * ( MAX_SERVO - MIN_SERVO ) + MIN_SERVO);

  if ( angleTete != oldAngleTete )
  {
    myServoTete.write( angleTete );
    oldAngleTete = angleTete;
  }
  
  int angleBrasGauche = (int) (( (float) nunchuk.analogY / ( MAX_JOY - MIN_JOY ) ) * ( MAX_SERVO - MIN_SERVO ) + MIN_SERVO);
  if ( angleBrasGauche != oldAngleBrasGauche )
  {
    myServoBrasGauche.write( angleBrasGauche );
    myServoBrasDroit.write( MAX_SERVO - angleBrasGauche );
    oldAngleBrasGauche = angleBrasGauche;
  }
  
  int statusBoutonZ = nunchuk.zButton;
  
  if ( oldStatusBoutonZ != statusBoutonZ )
  {
    digitalWrite( LED_GAUCHE, (statusBoutonZ == 1 ? HIGH : LOW ) );
    digitalWrite( LED_DROITE, (statusBoutonZ == 1 ? HIGH : LOW ) );
    digitalWrite( LED_PECTORAL, (statusBoutonZ == 1 ? HIGH : LOW ) );    
    oldStatusBoutonZ = statusBoutonZ;
  }
}


